﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Reinforcement;
namespace SAUlot
{
    public class AirplaneModel: CCEnvironment
    { 
       
        public event CCActionHandler CCMoveEvent;
        private const double h = 0.02;

        double alpha;
        double q;
        double theta;

        public AirplaneModel()
        {
            CCStartInRandomState();
        }

        public void CCStartInRandomState()
        {
            Random r = new Random();
            alpha = r.NextDouble()-0.5;
            q = 0;
            theta = r.NextDouble()-0.5;
            if (CCMoveEvent != null)
            {
                double[] result = new double[5];
                result[0] = alpha;
                result[1] = q;
                result[2] = theta;
                result[3] = 0;
                result[4] = 0;
                //rzucenie eventem
                CCMoveEvent(this, result);
            }
        }

        /** pobranie aktualnego stanu **/
        public void CCGetCurrentState(ref double[] state, ref double[] state_extension)
        {
            state[3] = alpha;
            state[2] = q;
            state[0] = theta;
        }

        public void CCMove(double[] decision_and_setpoint)
        {
            //wykonanie decyzji i obliczenie nowego stanu (nie modyfikuje stanu modelu)
            // zmienna_n1 = zmienna_(n+1)
            double alpha_n1 = AlphaRK(decision_and_setpoint[0]);
            double q_n1 = QRK(decision_and_setpoint[0]);
            double theta_n1 = ThetaRK();

            // zapisanie nowego stanu
            alpha = alpha_n1;
            q = q_n1;
            theta = theta_n1;

            //rzucenie eventem
            double[] result = new double[5];
            result[0] = alpha;
            result[1] = q;
            result[2] = -theta;
            result[3] = -decision_and_setpoint[0]; // decision
            result[4] = decision_and_setpoint[1]; // setpoint
            CCMoveEvent(this, result);
        }

        private double AlphaFunc(double a, double q, double d)
        {
            return -0.313 * a + 56.7 * q + 0.232 * d;
        }
        private double QFunc(double a, double q, double d)
        {
            return -0.0139 * a - 0.426 * q + 0.0203 * d;
        }
        private double ThetaFunc(double q)
        {
            return 56.7 * q;
        }
        private double AlphaRK(double decision)
        {
            double k1 = AlphaFunc(alpha, q, decision);
            double k2 = AlphaFunc(alpha + 0.5 * k1, q, decision);
            double k3 = AlphaFunc(alpha + 0.5 * k2, q, decision);
            double k4 = AlphaFunc(alpha + k3, q, decision);
            return alpha + (1 / 6.0) * (k1 + 2 * k2 + 2 * k3 + k4)*h;
        }
        private double QRK(double decision)
        {
            double k1 = QFunc(alpha, q, decision);
            double k2 = QFunc(alpha, q + 0.5 * k1, decision);
            double k3 = QFunc(alpha, q + 0.5 * k2, decision);
            double k4 = QFunc(alpha, q + k3, decision);
            return q + (1 / 6.0) * (k1 + 2 * k2 + 2 * k3 + k4)*h ;
        }
        private double ThetaRK()
        {
            double k1 = ThetaFunc(q);
            return theta+k1*h;
        }
    }
}
